#include "ros/ros.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "tf/transform_listener.h"
#include "std_srvs/Empty.h"

//Dieser Knoten sucht nach einem bestimmten ZeilMarker und published dessen Position als goal_pose für den FootstepPlanner

static tf::TransformListener* pListener = NULL;
ros::Publisher pub;

bool goalposeCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res) {
	tf::StampedTransform transform;
	try {
		pListener->lookupTransform("/map", "/nav_goal_desired", ros::Time(0), transform);
	} catch (tf::TransformException ex) {
		ROS_INFO("GoalPose not found, set goal manually");
		return false;
	}

	tf::Transform rotate;
	rotate.setOrigin(btVector3(0, 0, 0));
	rotate.setBasis(btMatrix3x3(0,-1,0,1,0,0,0,0,1));
	transform.mult(transform, rotate);
	geometry_msgs::PoseStamped pose;
	pose.pose.position.x = transform.getOrigin().x();
  	pose.pose.position.y = transform.getOrigin().y();
 	pose.pose.position.z = 0;
	pose.pose.orientation.x = transform.getRotation().x();
	pose.pose.orientation.y = transform.getRotation().y();
	pose.pose.orientation.z = transform.getRotation().z();
	pose.pose.orientation.w = transform.getRotation().w();
	pose.header.stamp = ros::Time::now();
	pose.header.frame_id = "/map";
	pub.publish(pose);
	return true;
}

int main(int argc, char **argv)
{
  /* init ROS */

  	ros::init(argc, argv, "biped_goalpose");
	ros::NodeHandle n;

	pub = n.advertise<geometry_msgs::PoseStamped>("goal2", 1000);

	pListener = new(tf::TransformListener);

	ros::ServiceServer localize_srv = n.advertiseService("goalpose_srv", goalposeCallback);

	ros::spin();


  return 0;
}

